/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Joystick;

/**
 * Thread that performs the actions when any buttons are pressed.
 * @author
 */
public class buttonThread extends Thread {

    private RobotMain mRobot997;
    private Joystick mLeftStick;
    private Joystick mRightStick;
    private Joystick mThirdStick;

    /**
     * The contructor.
     * @param A refernce to the 997 robot and its necessary objects.
     */
    public buttonThread(RobotMain robot) {
        mRobot997 = robot;
        mLeftStick = mRobot997.getLeftStick();
        mRightStick = mRobot997.getRightStick();
        mThirdStick = mRobot997.getThirdStick();
    }

    public void run() {
        while (mRobot997.isOperatorControl() && mRobot997.isEnabled()) {
            // DO NOT PUT ANY  CODE THAT TALKS TO THE MOTORS IN THIS THREAD!!!
            // All motor code should be in the main thread.

            //ExtendedKickerSensor stuff
            if(mRobot997.extendedKicker == mRobot997.Extended) {
                Timer.delay(1);
                mRobot997.retractKicker();
            }

            if(mThirdStick.getRawButton(8)){
                mRobot997.activateRetractKicker(true);
            }

            if(mThirdStick.getRawButton(9))
            {
                mRobot997.activateRetractKicker(false);
            }

            //************Right and Left stick code******************
            // Third stick code is below.
            if (mRightStick.getTrigger()) {
                mRobot997.performKick();
            }
            if (mRightStick.getRawButton(2)) {
                mRobot997.performSoftKick();
            }
            //if (mRightStick.getRawButton(3)) {
            //    mRobot997.sensorKick(mRightStick, 3);
            //}



            if (mRightStick.getRawButton(6)) {
                mRobot997.getLeftEncoder().reset();
                mRobot997.getRightEncoder().reset();
                mRobot997.getGyro().reset();
            }
            // Latch the kicker.
            if (mRightStick.getRawButton(6)) {
                mRobot997.driveLatchSolenoid(1);
                //Wait for button to be released.
                while (mRightStick.getRawButton(6)) {
                    Thread.yield();
                }
            }
            //Unlatch the kicker.
            if (mRightStick.getRawButton(7)) {
                mRobot997.driveLatchSolenoid(0);
                //Wait for button to be released.
                while (mRightStick.getRawButton(7)) {
                    Thread.yield();
                }
            }

            // Flush the kicker.
            if (mRightStick.getRawButton(10)) {
                mRobot997.driveShooterSolenoid(0);
                //Wait for button to be released.
                while (mRightStick.getRawButton(10)) {
                    Thread.yield();
                }
            }

            //Charge the Kicker.
            if (mRightStick.getRawButton(11)) {
                mRobot997.driveShooterSolenoid(1);
                //Wait for button to be released.
                while (mRightStick.getRawButton(11)) {
                    Thread.yield();
                }
            }




            //************Third stick code******************
            // Button 8 is used for pointing at vision target.
            if (mThirdStick.getTrigger()) {
                mRobot997.performKick();
            }
            if (mThirdStick.getRawButton(2)) {
                mRobot997.performSoftKick();
            }

            // Latch the kicker.
            if (mThirdStick.getRawButton(6)) {
                mRobot997.driveLatchSolenoid(1);
                //Wait for button to be released.
                while (mThirdStick.getRawButton(6)) {
                    Thread.yield();
                }
            }
            //Unlatch the kicker.
            if (mThirdStick.getRawButton(7)) {
                mRobot997.driveLatchSolenoid(0);
                //Wait for button to be released.
                while (mThirdStick.getRawButton(7)) {
                    Thread.yield();
                }
            }

            // Flush the kicker.
            if (mThirdStick.getRawButton(10)) {
                mRobot997.driveShooterSolenoid(0);
                //Wait for button to be released.
                while (mThirdStick.getRawButton(10)) {
                    Thread.yield();
                }
            }

            //Charge the Kicker.
            if (mThirdStick.getRawButton(11)) {
                mRobot997.driveShooterSolenoid(1);
                //Wait for button to be released.
                while (mThirdStick.getRawButton(11)) {
                    Thread.yield();
                }
            }
//            if (mThirdStick.getRawButton(9)) {
 ///               mRobot997.sensorKick(mThirdStick, 9);
    //        }


            Thread.yield();
            Timer.delay(0.0005);
        }
    }
}


